#include <remoter.hpp>
#include <settings.hpp>
#include <tcp_client.hpp>
#include <utils.hpp>

static Remoter* s_Remoter = nullptr;

Remoter* Remoter::Instance()
{
	if (s_Remoter == nullptr)
	{
		s_Remoter = new Remoter();
	}
	return s_Remoter;
}

Remoter::Remoter()
	: WorkQueue(PRIORITY_WQ_HIGH)
	, m_PubRc("msg_rc_s", sizeof(msg_rc_s))
	, m_BtnVal(0)
{
	m_UdpSender = new UdpSender(SERVER_IP, UDP_PORT_SND_RC);

	memset(&m_Rc, 0, sizeof(msg_rc_s));
	memset(&m_RcRaw, 0, sizeof(msg_rc_raw_s));
	m_RcObj.Init();
	m_Btn.Init();

	m_CaliState = E_RC_CALI_NORMAL;

	m_RcCali[E_RC_CALI_NORMAL]		= new RemoterCaliNormal(E_RC_CALI_NORMAL);
	m_RcCali[E_RC_CALI_PREPARE]		= new RemoterCaliPrepare(E_RC_CALI_PREPARE);
	m_RcCali[E_RC_CALI_THRO_UP]		= new RemoterCaliThroUp(E_RC_CALI_THRO_UP);
	m_RcCali[E_RC_CALI_THRO_DOWN]	= new RemoterCaliThroDown(E_RC_CALI_THRO_DOWN);
	m_RcCali[E_RC_CALI_ROLL_LEFT]	= new RemoterCaliRollLeft(E_RC_CALI_ROLL_LEFT);
	m_RcCali[E_RC_CALI_ROLL_RIGHT]	= new RemoterCaliRollRight(E_RC_CALI_ROLL_RIGHT);
	m_RcCali[E_RC_CALI_PITCH_FRONT] = new RemoterCaliPitchFront(E_RC_CALI_PITCH_FRONT);
	m_RcCali[E_RC_CALI_PITCH_BACK]	= new RemoterCaliPitchBack(E_RC_CALI_PITCH_BACK);
	m_RcCali[E_RC_CALI_YAW_LEFT]	= new RemoterCaliYawLeft(E_RC_CALI_YAW_LEFT);
	m_RcCali[E_RC_CALI_YAW_RIGHT]	= new RemoterCaliYawRight(E_RC_CALI_YAW_RIGHT);
	m_RcCali[E_RC_CALI_FINISHED]	= new RemoterCaliFinished(E_RC_CALI_FINISHED);

	m_RcCaliCurr = m_RcCali[E_RC_CALI_NORMAL];
}

Remoter::~Remoter()
{
}

void Remoter::SwitchCaliState(int state)
{
	m_CaliState	 = state;
	m_CaliState	 = LimitInt(m_CaliState, E_RC_CALI_NORMAL, E_RC_CALI_FINISHED);
	m_RcCaliCurr = m_RcCali[m_CaliState];
	m_RcCaliCurr->Reset();
}

void Remoter::SwitchCaliNext(void)
{
	int state = m_CaliState + 1;
	state %= E_RC_CALI_CNT;
	SwitchCaliState(state);
}

int Remoter::GetCaliState(void)
{
	return m_CaliState;
}

void Remoter::RunImpl(void* arg)
{
	g_Settings->rc_ind_thro	 = 3;
	g_Settings->rc_ind_roll	 = 0;
	g_Settings->rc_ind_pitch = 1;
	g_Settings->rc_ind_yaw	 = 2;

	g_Settings->rc_cali_thro_max	 = 336;
	g_Settings->rc_cali_thro_mid	 = 3901;
	g_Settings->rc_cali_thro_min	 = 3901;
	g_Settings->rc_cali_thro_reverse = 1;

	g_Settings->rc_cali_roll_max	 = 295;
	g_Settings->rc_cali_roll_mid	 = 1942;
	g_Settings->rc_cali_roll_min	 = 3971;
	g_Settings->rc_cali_roll_reverse = 1;

	g_Settings->rc_cali_pitch_max	  = 291;
	g_Settings->rc_cali_pitch_mid	  = 2035;
	g_Settings->rc_cali_pitch_min	  = 3979;
	g_Settings->rc_cali_pitch_reverse = 1;

	g_Settings->rc_cali_yaw_max		= 308;
	g_Settings->rc_cali_yaw_mid		= 1997;
	g_Settings->rc_cali_yaw_min		= 3949;
	g_Settings->rc_cali_yaw_reverse = 1;

	m_RcObj.RunImpl(&m_RcRaw);
	m_Btn.RunImpl(m_BtnVal);

	m_RcCaliCurr->Process(&m_RcRaw, &m_Rc);
	m_Rc.sw_btn = m_BtnVal;
	m_Rc.valid	= true;

	m_PubRc.Publish(&m_Rc, sizeof(msg_rc_s));
	m_UdpSender->SendData(P_TYPE_RC, (char*)&m_Rc, sizeof(msg_rc_s));
}

/* @RegistMainFunction */
int remoter_main(int argc, char** argv)
{
	if (argc == 3 && strcmp(argv[1], "state_set") == 0)
	{
		int state		  = strtol(argv[2], NULL, 0);
		Remoter* pRemoter = Remoter::Instance();
		pRemoter->SwitchCaliState(state);
		return 0;
	}
	if (argc == 2 && strcmp(argv[1], "state_get") == 0)
	{
		Remoter* pRemoter = Remoter::Instance();
		int state		  = pRemoter->GetCaliState();
		LOG("Remoter state = %d\n", state);
		return 0;
	}

	LOG("remoter cmd error.\n");

	return -1;
}